/******************************************************************************
 *
 * MotionViewer
 * Copyright (C) 2007 Sangyoon Lee, Electronic Visualization Laboratory, 
 * University of Illinois at Chicago
 *
 * This software is free software; you can redistribute it and/or modify it 
 * under the terms of the GNU Lesser General Public License as published by
 * the Free Software Foundation; either Version 2.1 of the License, or 
 * (at your option) any later version.
 *
 * This software is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
 * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU Lesser General Public 
 * License for more details.
 * 
 * You should have received a copy of the GNU Lesser Public License along
 * with this software; if not, write to the Free Software Foundation, Inc., 
 * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
 *
 * Questions or comments about MotionViewer should be directed to 
 * cavern@evl.uic.edu
 *
 *****************************************************************************/
#include <ogreplatform.h>
#if OGRE_PLATFORM == OGRE_PLATFORM_APPLE
#include <Carbon/Carbon.h>
#endif

#include "ViconCharacter.h"
#include "ViconMotionDataSet.h"

#pragma warning(disable:4267)	
#pragma warning(disable:4018)	

static int g_VCID = -1;

ViconCharacter::ViconCharacter(SceneManager* mgr, SceneNode* head)
{
	m_pSceneMgr = mgr;
	m_pHeadNode = head;

	m_pDataSet	= NULL;
	m_pEntity	= NULL;
	m_pBaseNode	= NULL;
	m_pSkeleton	= NULL;
	m_pAnimState = NULL;

	m_iID = ++g_VCID;
}

ViconCharacter::~ViconCharacter()
{
}

bool ViconCharacter::createCharacter(const char* filename)
{
	// make unique name
	char name[64], manual[64];
	sprintf(name, "character_%i", m_iID);
	sprintf(manual, "LLmanual_%i", m_iID);
	m_sName = std::string(name);

	std::string fullname(filename);
	std::string nameonly(filename);
	
#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32	
	int pos = fullname.find_last_of("\\");
#else
	int pos = fullname.find_last_of("/");
#endif
	
	if(pos != fullname.npos)
	{
		std::string path = fullname.substr(0, pos);
		ResourceGroupManager::getSingleton().addResourceLocation(path.c_str(), "FileSystem", filename);
		ResourceGroupManager::getSingleton().initialiseAllResourceGroups();
		ResourceGroupManager::getSingleton().loadResourceGroup(ResourceGroupManager::getSingleton().getWorldResourceGroupName(),false, true);

		nameonly = fullname.substr(pos+1);
	}
	
	//const char* tstr = nameonly.c_str();	// not using right now
	MeshPtr mesh = MeshManager::getSingleton().load(nameonly.c_str(),ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);

	// verify character...
	if (!mesh->hasSkeleton())
	{
		g_VCID--;
		return false;
	}

	m_pEntity = m_pSceneMgr->createEntity(name, nameonly.c_str());
	m_pBaseNode = m_pHeadNode->createChildSceneNode(name);
	m_pBaseNode->attachObject(m_pEntity);
	
	m_pSkeleton = m_pEntity->getSkeleton();
	if (m_pSkeleton == NULL)
		return false;
	
	int num = m_pSkeleton->getNumBones();

	Bone* pBone;
	for (int i=0; i<num; i++)
	{
		pBone = m_pSkeleton->getBone(i);
		pBone->setManuallyControlled(true);
	}
	
	
	return true;
}

void ViconCharacter::connectToMotionData(ViconMotionDataSet* dataset)
{
	if (m_pDataSet)
		disconnectMotionData();

	m_pDataSet = dataset;
}

void ViconCharacter::disconnectMotionData()
{
	m_pDataSet = NULL;
	m_vBoneMap.clear();

	// reset bone matrix
}

void ViconCharacter::loadSkeletonMapping(const char* mapfile)
{
	if (m_pDataSet == NULL)
		return;

	// read mapfile and build mapping vector

	char line[128];
	int imapping = 0;
	std::fstream mf(mapfile,std::ios::in);

	if (!mf)
		return;

	while(!mf.eof())
	{
		mf.getline(line, 128);
		if (line[0] == '/' && line[1] == '/')
			continue;
		else
			imapping++;
	}

	if (imapping != 0)
	{
		mf.clear();
		mf.seekg(0,std::fstream::beg);
		std::vector<int> bm(m_pDataSet->getNumBodies(), -1);
		m_vBoneMap = bm;

		char seps[] = " .\t\n\r";
		char* token;
		int ibody;
		int ibone;
		Bone* pBone;

		while(!mf.eof())
		{
			ibody = ibone = -1;
			pBone = NULL;

			mf.getline(line, 128);

			if (line[0] == '/' && line[1] == '/')
				continue;
			else
			{
				token = strtok(line, seps);	
				if (token == NULL)
					continue;
				ibody = m_pDataSet->getBodyIndex(token);
				
				token = strtok(NULL, seps);
				if (token == NULL)
					continue;
				pBone = m_pSkeleton->getBone(token);
				
				if (pBone == NULL)
					continue;

				ibone = pBone->getHandle();

				if (ibody !=-1 && ibody < m_vBoneMap.size())
				{
					m_vBoneMap[ibody] = ibone;
				}
			}
		}
	}

	mf.close();

	std::string str(mapfile);

#if OGRE_PLATFORM == OGRE_PLATFORM_WIN32	
	int pos = str.find_last_of("\\");
#else
	int pos = str.find_last_of("/");
#endif

	str = str.substr(pos+1);
	m_sMapFileName;

}

void ViconCharacter::update(float addedTime)
{
	if (m_pDataSet == NULL || m_vBoneMap.empty())
		return;

	if (m_pAnimState)
	{
		m_pAnimState->getParent()->_notifyDirty();
		m_pAnimState->addTime(addedTime);
	}

	// update bone matrix
	updateSkeleton();

}

void ViconCharacter::updateSkeleton()
{

	float qt[4], pos[3];
	Bone* pBone;
	for (int i=0; i< m_vBoneMap.size(); i++)
	{
		if (m_vBoneMap[i] == -1)
			continue;

		m_pDataSet->getBodyQuaternion(i, qt, false);
		pBone = m_pSkeleton->getBone(m_vBoneMap[i]);
		
		if (pBone == NULL)
			continue;
		
		if (m_vBoneMap[i] == 0)
		{
			// apply rootbone translation
			m_pDataSet->getBodyTranslation(i, pos);
			pBone->setPosition(pos[0], pos[1], pos[2]);
		}

		pBone->setOrientation(Ogre::Quaternion(qt[0], qt[1], qt[2], qt[3]));
	}

}

void ViconCharacter::setPosition(float x, float y, float z)
{
	if (m_pBaseNode == NULL)
		return;

	m_pBaseNode->setPosition(x, y, z);
}

void ViconCharacter::getPosition(float* pos)
{
	if (m_pBaseNode == NULL)
		return;

	Ogre::Vector3 vec = m_pBaseNode->getPosition();
	pos[0] = vec[0];
	pos[1] = vec[1];
	pos[2] = vec[2];

}

unsigned int ViconCharacter::getNumBones()
{
	if (m_pSkeleton == NULL)
		return 0;
	
	return m_pSkeleton->getNumBones();
}
